/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/*!******************************************************************
 @author     ZhangWanjie
 ********************************************************************/
#include "ZZUMission.h"

static int nRobotID = 5;
static bool bFirstMove = true;
static bool bGrab = true;
static int nFind = -1;

CZZUMission::CZZUMission()
{
}

CZZUMission::~CZZUMission()
{
}

/**************************************************
 * 任务系统初始化
 * ************************************************/
void CZZUMission::Start()
{
    // [1] 单向
    RobotPathToTask(5,"5");

    // [2] 回环
    //Signal_3();

    bPause = false;
}

/**************************************************
 * 任务切换
 * ************************************************/
void CZZUMission::MissionComplete(stMission* inMission)
{
    ROS_WARN("[CZZUMission] Mission任务 \"%s\" 执行完毕!! ",inMission->mission.c_str());
    
    ostringstream oss;
    oss << nRobotID<<"号循线去往航点"<<nRobotID;
    nFind = inMission->mission.find(oss.str());
    if(nFind >= 0)
    {
        // 【1】抓取阶段
        if(bGrab == true)
        {
            if(nRobotID > 1)
            { 
                GrabPalletRack(nRobotID);
                if(bFirstMove == true)
                {
                    ostringstream oss_1;
                    oss_1 << (nRobotID + 1);
                    RobotPathToTask(nRobotID,oss_1.str());
                    PlacePalletRack(nRobotID);
                    nRobotID --;
                    // 下一个机器人到自己货架
                    ostringstream oss_2;
                    oss_2 << nRobotID;
                    RobotPathToTask(nRobotID,oss_2.str());
                    GrabPalletRack(nRobotID);
                }
            }
            else
            {
                // 此时1号抓抓取完毕，其他人应该都抓取放置完毕了
                RobotPathToTask(1,"2");
                PlacePalletRack(1);
                RobotPathToTask(1,"1");
                nRobotID = 1;
                bGrab = false;  // 抓取放置阶段结束
            }
        }
        else
        {
            // 抓取的回位阶段
            nRobotID ++;
            ostringstream oss_3;
            oss_3 << nRobotID;
            RobotPathToTask(nRobotID,oss_3.str());
            
            if(nRobotID == 5)
            {
                bGrab = true;  // 恢复到抓取放置阶段
            }
        }
    }

    ostringstream oss_4;
    oss_4 << nRobotID<<"号抓取Pallet";
    nFind = inMission->mission.find(oss_4.str());
    if(nFind >= 0)
    {
        if(nRobotID == 1 && bFirstMove == true )
        {
            bFirstMove = false;
            return;
        }
        if(bFirstMove == false)
        {
            if(nRobotID == 1)
            {
                RobotPathToTask(1,"2");
                PlacePalletRack(1);
                RobotPathToTask(1,"1");
                bGrab = false;  // 抓取放置阶段结束
            }
            else
            {
                ostringstream oss_1;
                oss_1 << (nRobotID + 1);
                RobotPathToTask(nRobotID,oss_1.str());
                PlacePalletRack(nRobotID);
                nRobotID --;
                GrabPalletRack(nRobotID);
            }
        }
    }

    nFind = inMission->mission.find("全员放置Pallet");
    if(nFind >= 0)
    {
       Signal_3();
    }

}

// 信号1
void CZZUMission::Signal_1()
{
    RobotPathToTask(1,"1");
    GrabPalletRack(1);
    RobotPathToTask(1,"2");
    PlacePalletRack(1);
    bPause = false;
}

// 信号2
void CZZUMission::Signal_2()
{
    RobotPathToTask(1,"1");
    RobotPathToTask(2,"2");
    RobotPathToTask(3,"3");
    RobotPathToTask(4,"4");
    RobotPathToTask(5,"5");
    AllGrabPallet();
    RobotPathToTask(1,"2");
    RobotPathToTask(2,"3");
    RobotPathToTask(3,"4");
    RobotPathToTask(4,"5");
    RobotPathToTask(5,"1");
    AllPlacePallet();
}

// 信号3
void CZZUMission::Signal_3()
{
    // RobotsSyncPathTo(3,"4",  4, "3");
    // RobotsSyncPathTo(3,"3",  4, "4");
    RobotPathToTask(1,"1");
    RobotPathToTask(2,"2");
    RobotPathToTask(3,"3");
    RobotPathToTask(4,"4");
    RobotPathToTask(5,"5");
    for(int i=1;i<6;i++)
        GrabPalletRack(i);
    AllPathTo("2" , "3" , "4" , "5" , "1");
    AllPlacePallet();
}

void CZZUMission::Tac_1()
{

}